/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#pragma once

#include "Chain.h"

namespace RobotPlant { namespace Mechanism
{
	using namespace RobotPlant::Math;

	/*
	*/
	class RP_MECHANISM_API Con_ComputedTorgue
	{
	public:
		Con_ComputedTorgue(const Chain* pRobot, const ColumnVector & Kp, const ColumnVector & Kd);
		void Set_Kd(const ColumnVector & Kd);
		void Set_Kp(const ColumnVector & Kp);

		/*
		@brief: calculates joint acceleration.
		@param: pd: desired joint value.
		@param: ppd: desired joint velocity.
		@param: pppd: desired joint acceleration.
		@returns: joint acceleration.
		*/
		ColumnVector Cmd_JointAcc(const ColumnVector & qd, const ColumnVector & qpd, const ColumnVector & qppd);

		/*
		@brief: calculates joint acceleration.
		@param: pd: desired joint value.
		@param: ppd: desired joint velocity.
		@param: pppd: desired joint acceleration.
		@param: grav: gravity w.r.t robot's base frame.
		@returns: joint torque.
		*/
		ColumnVector Cmd_JointTorque(const ColumnVector & qd, const ColumnVector & qpd, const ColumnVector & qppd, const Vector3 & grav);

	private:
		const Chain *_pRobot;            //!< Degree of freedom.

		Matrix _Kp;  //!< Position error gain.
		Matrix _Kd;  //!< Velocity error gain.
	};



}}